clc
clear
%% 初始化 %%
sigma_s = 0.001;% 站址误差
sigma_theta = 0.001;% 方位角测量误差
sigma_fi = 0.001;% 高低角测量误差
% 目标/测向站 实际位置
T = [20 20 20];
S1 = [10 0 0];
S2 = [-10 0 0];
S3 = [0 -30 0];
% 随机 K个点
K = 15;
X = zeros(K,1);
Y = zeros(K,1);
Z = zeros(K,1);
%% 条件1 %%
for i= 1:K
    [X(i),Y(i),Z(i)] = P_estimate(T,S1,S2,S3,sigma_s,sigma_theta,sigma_fi);
end
% 将测向站坐标也加上
% x1 = 10; x2 = -10; x3 = 0;
% y1 = 0; y2 = 0; y3 = -30;
% z1 = 0; z2 = 0; z3 = 0;
% X(K+1:K+4) = [x1 x2 x3 40];
% Y(K+1:K+4) = [y1 y2 y3 10];
% Z(K+1:K+4) = [z1 z2 z3 10];
scatter3(X,Y,Z);
xlim([19 21]);ylim([19 21]);zlim([19 21]);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot3(T(1),T(2),T(3),'*');
%% 增加站址误差 %%
sigma_s = 0.01;
for i= 1:K
    [X(i),Y(i),Z(i)] = P_estimate(T,S1,S2,S3,sigma_s,sigma_theta,sigma_fi);
end
figure
scatter3(X,Y,Z);
xlim([19 21]);ylim([19 21]);zlim([19 21]);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot3(T(1),T(2),T(3),'*');
%% 增加方位角误差 %%
sigma_theta = 0.01;
sigma_s = 0.001;
for i= 1:K
    [X(i),Y(i),Z(i)] = P_estimate(T,S1,S2,S3,sigma_s,sigma_theta,sigma_fi);
end
figure
scatter3(X,Y,Z);
xlim([19 21]);ylim([19 21]);zlim([19 21]);
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
title([' \sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot3(T(1),T(2),T(3),'*');
%% 增加高低角误差 %%
sigma_fi = 0.01;
sigma_theta = 0.001;
sigma_s = 0.001;
for i= 1:K
    [X(i),Y(i),Z(i)] = P_estimate(T,S1,S2,S3,sigma_s,sigma_theta,sigma_fi);
end
figure
scatter3(X,Y,Z);
xlim([19 21]);ylim([19 21]);zlim([19 21])
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot3(T(1),T(2),T(3),'*');


